#include "../_includes.h"

extern int timeout_toarm;

void auto_right_1()
{
  gyro_reset();
  get_ring();
  thread t_hold(hold_inside);//吸入圈后及时停
  stable_go(720, 100, 500, 1500);//走长距离
  turn_pid_adjust_agl_absolute(90, 100, 800, Block::yes);
  stop_getring();
  stable_go(-400,100,100,800);
  chassis.stable_forward_dist_relative(-100,100,400);//走短距离
  clamp.set(true);

 
  t_hold.interrupt();
  wait(100);
  turn_pid_adjust_agl_absolute(0, 100, 800, Block::yes);
  get_ring();
  // arm.act_for_enc_absolute(400, 100, 800, Block::no);
  stable_go(750, 100, 500, 1000);
  chassis.stable_forward_dist_relative(300, 100, 500);
  wait(200);

  


  //取消的方案
  // sucklock.set(true);
  // turn_pid_adjust_agl_absolute(0, 100, 800, Block::yes);
  // chassis.stable_forward_dist_relative(200, 100, 500);
  // sucklock.set(false);
  // // 搓的方案
  // thread t_twitch(twitch);
  // wait(2000);
  // t_twitch.interrupt();
  //这里之后待修改--准备到wall stake

  turn_pid_adjust_agl_absolute(165, 100, 1500, Block::yes);   
  stable_go(425, 100, 500, 1500);//走长距离把535改成470
  turn_pid_adjust_agl_absolute(270,100,1000,Block::yes);
  //取单环方案--
  arm.act_for_enc_absolute(0, 100, 800, Block::no);
  timeout_toarm = 3000;
  thread t_toarm(auto_arm_holdinside_blue);
  chassis.stable_forward_dist_relative(210,100,800);
  wait(300);
  for (int i = 0; i < 1; i++)
  {
    chassis.stable_forward_dist_relative(-100, 70, 500);
    chassis.stable_forward_dist_relative(100, 70, 500);

  }
  wait(400);//为了确保环到机械臂
  chassis.stable_forward_dist_relative(163, 70, 500);
  wait(200);
  chassis.stable_forward_dist_relative(-163, 70, 500);
  arm.act_for_enc_absolute(1480,100,1500,Block::yes);
  arm.stop(hold);
  wait(100);
  chassis.stable_forward_dist_relative(210,100,1000);
  arm.act_for_enc_absolute(500,70,1500,Block::yes);
  sucklock.set(true);
  t_toarm.interrupt();
  out_ring();
  wait(100);
  chassis.stable_forward_dist_relative(-300,100,500);
  stop_getring();
  turn_pid_adjust_agl_absolute(180,100,1000,Block::yes);
  sucklock.set(false);
  get_ring();
  stable_go(900, 80, 500, 1500);
  arm.act_for_enc_absolute(800,100,1000,Block::no);
  chassis.stable_forward_dist_relative(155,100,800);//把160改为155
  wait(100);
  //准备取角落圈
  turn_pid_adjust_agl_absolute(222,100,1000,Block::yes);
  sucklock.set(true);
  chassis.stable_forward_dist_relative(600,100,1000);
  sucklock.set(false);
  wait(200);
  for (int i = 0; i < 2; i++)
  {
    chassis.stable_forward_dist_relative(-100, 70, 500);
    chassis.stable_forward_dist_relative(120, 70, 500);
  }
  wait(200);
  //准备放墩子
  chassis.stable_forward_dist_relative(-310, 70, 500);
  arm.act_for_enc_absolute(0,100,1000,Block::yes);
  stop_getring();
  turn_pid_adjust_agl_absolute(45,100,1000,Block::yes);
  chassis.stable_forward_dist_relative(-314, 70, 1000);
  wait(100);
  clamp.set(false);
  chassis.stable_forward_dist_relative(60, 30, 100);
  //准备下一个wall stake
  chassis.stable_forward_dist_relative(310, 70, 900);
  turn_pid_adjust_agl_absolute(90,100,1000,Block::yes);
  chassis.stable_forward_dist_relative(-480, 70, 1000);
  stable_go(1280, 90, 500, 1700);
  get_ring();
  timeout_toarm = 3000;
  thread t_toarm2(auto_toarm);
  chassis.stable_forward_dist_relative(350, 80, 800);
  for (int i = 0; i < 1; i++)
  {
    chassis.stable_forward_dist_relative(-100, 70, 500);
    chassis.stable_forward_dist_relative(100, 70, 500);
  }
  wait(900);
  t_toarm2.interrupt();
  stop_getring();
  turn_pid_adjust_agl_absolute(180,100,1000,Block::yes);
  arm.act_for_enc_absolute(1000,100,1500,Block::yes);
  arm.stop(hold);
  chassis.stable_forward_dist_relative(185,100,500);
  arm.act_for_enc_absolute(400,70,1300,Block::yes);
  if(arm.get_enc()>460){
    arm.act_for_enc_absolute(1000,100,1500,Block::yes);
    chassis.stable_forward_dist_relative(-198,100,500);
  }
  else{
    chassis.stable_forward_dist_relative(-198,100,500);
  }
  //准备进另外一个半区
  turn_pid_adjust_agl_absolute(225,100,1000,Block::yes);
  arm.act_for_enc_absolute(0,100,1100,Block::no);
  stable_go(-812,100,100,800);
  turn_pid_adjust_agl_absolute(270,100,1000,Block::yes);
  stop_getring();
  stable_go(-500,100,100,800);
  clamp.set(true);
  chassis.stable_forward_dist_relative(-85,100,400);//走短距离
  //前面基本确定不用太改
  //取到墩子
  turn_pid_adjust_agl_absolute(360,90,1100,Block::yes);
  get_ring();
  stable_go(610, 90, 500, 1000);
  //准备取双环
  stable_go(-680, 90, 500, 1000);
  turn_pid_adjust_agl_absolute(387,100,1000,Block::yes);


  //取双环到桩子只取红环
  stable_go(350, 90, 500, 1000);
  chassis.stable_forward_dist_relative(200, 100, 500);
  stable_go(-453, 90, 500, 1000);
  turn_pid_adjust_agl_absolute(540,100,1000,Block::yes);
  arm.act_for_enc_absolute(800,100,1000,Block::no);
  stable_go(624, 90, 500, 1300);
  //准备取角落圈
  turn_pid_adjust_agl_absolute(498,100,1000,Block::yes);

  sucklock.set(true);
  chassis.stable_forward_dist_relative(500,100,700);
  sucklock.set(false);
  wait(200);
  for (int i = 0; i < 1; i++)
  {
    chassis.stable_forward_dist_relative(-100, 70, 500);
    chassis.stable_forward_dist_relative(110, 70, 500);
  }
  wait(200);
  //准备放墩子
  chassis.stable_forward_dist_relative(-310, 70, 500);
  arm.act_for_enc_absolute(0,100,1000,Block::yes);
  stop_getring();
  turn_pid_adjust_agl_absolute(315,100,1000,Block::yes);
  chassis.stable_forward_dist_relative(-330, 70, 1000);
  clamp.set(false);
  chassis.stable_forward_dist_relative(50, 70, 400);
  turn_pid_adjust_agl_absolute(315,100,300,Block::yes);
  hanger.set(true);
  stable_go(1700, 100, 500, 1300);
  hanger.set(false);



































  
//暂时不用的方案
// //和左边同样的方式
//   arm.act_for_enc_absolute(0, 100, 800, Block::no);
//   chassis.stable_forward_dist_relative(240,100,800);
//   wait(500);

//   timeout_toarm = 3000;
//   thread t_toarm3(auto_toarm);
//   wait(300);
//   for (int i = 0; i < 1; i++)
//   {
//     chassis.stable_forward_dist_relative(-100, 70, 500);
//     chassis.stable_forward_dist_relative(100, 70, 500);

//   }
//   wait(600);//为了确保环到机械臂
//   t_toarm3.interrupt();
//   wait(100);
//   thread t_to_out1(auto_holdinside_blue);
//   chassis.stable_forward_dist_relative(130, 70, 500);
//   chassis.stable_forward_dist_relative(-100, 70, 500);
//   turn_pid_adjust_agl_absolute(450,100,1000,Block::yes);
//   chassis.stable_forward_dist_relative(-160,100,800);
//   arm.act_for_enc_absolute(1480,100,1500,Block::yes);
//   arm.stop(hold);
//   chassis.stable_forward_dist_relative(210,100,1000);
//   arm.act_for_enc_absolute(600,100,1000,Block::yes);
//   chassis.stable_forward_dist_relative(-300,100,500);
//   turn_pid_adjust_agl_absolute(540,100,1000,Block::yes);
//   arm.act_for_enc_absolute(0,100,1000,Block::yes);
//   get_ring();
//   stable_go(900, 80, 500, 1500);
//   chassis.stable_forward_dist_relative(120,100,500);
//   wait(400);



  
}
